from tls01 import TLS01
import pyb_message
import MyMotor
mylocation=0
status='stop'
what_todo='take_first_box'
saved_box=[['0','0'],['0','0']]
storage_cache=[['0','0'],['0','0'],['0','0'],['0','0'],['0','0'],['0','0']]
stacking_area=['0','0','0']

BLE = TLS01(1, 9600)  # 设置串口号3和波特率, TX--B6, RX--B7

def detect_box():
    pyb_message.send_text('g')
    box=pyb_message.receive_text()
    if box=='1':
        storage_cache[mylocation][0]='red'
        storage_cache[mylocation][1]='cricle'
        return ('red','cricle')
    elif box=='2':
        storage_cache[mylocation][0] = 'red'
        storage_cache[mylocation][1] = 'rectangel'
        return ('red','rectangel')
    elif box=='3':
        storage_cache[mylocation][0] = 'red'
        storage_cache[mylocation][1] = 'triangel'
        return ('red','triangel')
    elif box=='4':
        storage_cache[mylocation][0] = 'blue'
        storage_cache[mylocation][1] = 'cricle'
        return ('blue','cricle')
    elif box=='5':
        storage_cache[mylocation][0] = 'blue'
        storage_cache[mylocation][1] = 'rectangel'
        return ('blue','rectangel')
    elif box=='6':
        storage_cache[mylocation][0] = 'blue'
        storage_cache[mylocation][1] = 'triangel'
        return ('blue', 'triangel')


def go_to_location(wish_location):
    while abs((wish_location-mylocation)) != 0 :
        if wish_location > mylocation :
            MyMotor.Tracking(BLE)
            mylocation = mylocation + 1
        elif wish_location < mylocation :
            MyMotor.TrackingB(BLE)
            mylocation = mylocation - 1



def main():
    global what_todo
    global mylocation
    while 1 :
        if stacking_area[0]=='0' or stacking_area[1]=='0' or stacking_area[2]=='0' :
            if what_todo == 'take_first_box' :
                for i in storage_cache :
                    if i[0]=='red' :
                        go_to_location(storage_cache.index(i))
                        MyMotor.take_box()
                        what_todo='take_second_box'
                MyMotor.Tracking(BLE)
                mylocation = mylocation + 1
                detect_box()
                print(storage_cache)
                return 0
            elif what_todo == 'take_second_dox':
                for i in storage_cache :
                    if i[1]==saved_box[0][1]:
                        go_to_location(storage_cache.index(i))
                        MyMotor.tack_box()
                        what_todo='laydown_box'
                        break
                MyMotor.Tracking()
                detect_box()
            elif what_todo == 'laydown_box':
                go_to_location()
                text=pyb_message.receive_text()
                if text == 's':
                    pyb_message.send_text('laydown_box')
                    text=pyb_message.receive_text()
                    if text == 's':
                        what_todo = 'take_first_box'
                        pass

if __name__ == '__main__':
    main()